% First exercise - Single target tracking
% Generate clutter and single target track in 0<x,y<10km

%clear all;
%close all;
%random_seed = 38;
%sched_mode = 1;

% random seed 30 mode 1: Target goes beyond 90 degress 
% (-90 for the other radar) but is still in beam so gets scheduled. 
% Should probably delete the intercept point from the schedule and
% indicated that the target was scheduled in any case.

duration_sec = 1 * 60; % seconds?

if ~exist('random_seed', 'var')
  random_seed = 1;
end

s = RandStream.create('mt19937ar','seed',random_seed);
RandStream.setGlobalStream(s);

sched_algorithm = 'SG';
sched_max_iterations = 1;
% Maybe change the way these propogate into the scheduling?
param.schedule.algorithm = sched_algorithm;
param.schedule.max_iterations = sched_max_iterations;

if ~exist('sched_mode', 'var')
  sched_mode = 3;
end
  
switch sched_mode
  case 1 % No high-level prediction
    predict_intercepts_upper = 0;
    predict_intercepts_lower = 1;
    nimble_scheduling = 0;
    
  case 2 % Simple prediction
    predict_intercepts_upper = 1;
    predict_intercepts_lower = 1;
    nimble_scheduling = 0;
    
  case 3 % Nimble
    predict_intercepts_upper = 1;
    predict_intercepts_lower = 1;
    nimble_scheduling = 1;
end

if ~exist('plot_on', 'var')
  plot_on = 1;
end

if plot_on
  % Figure handle
  f1 = figure(1);
end

if ~exist('sim_mode', 'var')
  sim_mode = 1;
end

switch sim_mode
  case 1 % default
    initial_tsv = [0 0 25 25];
    sigma_a_sim = 2;
  case 2 % fase initial velocity    
    initial_tsv = [0 0 50 50];
    sigma_a_sim = 2;
  case 3 % fast accelerations
    initial_tsv = [0 0 25 25];
    sigma_a_sim = 20;
end

%% True Target Data Generation

% Basic fundamentals
max_range = 1e4;
beam_width = 1;
scan_angle = 90; % Cannot easily change this
num_beams = 1;
min_beam_increment = 1; % vary between (0 and 10] % Not working ~= num_beams

iter_per_sec = ceil(scan_angle/(beam_width*min_beam_increment/num_beams));
beam_az_prog = (beam_width*min_beam_increment/num_beams);
duration = ceil(duration_sec * iter_per_sec);

% Calculated
range_extent = [0 max_range;... 
                0 max_range]; % Tied to scan angle but not calculated

Vol         =  deg2rad(scan_angle) * max_range^2;
               %(range_extent(1,2)-range_extent(1,1))*...
               %(range_extent(2,2)-range_extent(2,1));
            
Pfa_cell    = 1e-6;
Range_res   = 3.75;
Range_bins  = max_range/Range_res; %1000;
Ang_bins    = ceil(scan_angle/(beam_width*min_beam_increment/num_beams))+1; %20; %720;
No_of_pulses_on_tgt = 32;
beta_FA     = No_of_pulses_on_tgt*Range_bins*Ang_bins*Pfa_cell/Vol;  % False alarm rate (number FA/area/scan)

T_t = 1/iter_per_sec; % Scan interval
C_t = T_t^2/2;

T_f = T_t; %1;
C_f = T_f^2/2;

P_D = 0.9;        % Probability of detection  

% Target setup
num_targets = 10;
num_radars = 2;

% Radar setup
radar_loc = [0 0; 0 1e4]';
radar_az = [0; 0];
radar_bw = [beam_width; beam_width]; % should be 2 * sigma_az (single beam but here assuming 10 beams)
radar_bp = [beam_az_prog; beam_az_prog];
radar_dir = [1; -1];
radar_range = [sqrt(2*(max_range^2)); sqrt(2*(max_range^2))];
radar_duty = 0.05;

radar_az_min = [0;-scan_angle];
radar_az_max = [scan_angle; 0];
     
% Tracking variables
%x     = [5e3 5e3 25 25]';
A_t     = [1 0 T_t 0;...
         0 1 0 T_t;...
         0 0 1 0;...
         0 0 0 1];
A_f     = [1 0 T_f 0;...
         0 1 0 T_f;...
         0 0 1 0;...
         0 0 0 1];
B_t     = [C_t 0;...
         0 C_t;...
         T_t 0;...
         0 T_t];
B_f     = [C_f 0;...
         0 C_f;...
         T_f 0;...
         0 T_f];
L     = [1 0;... % Kalman filter state init 
         0 1;... % matrix
         0 0;...
         0 0];
sigma_a     = sigma_a_sim; %2; %1e-20; %2;      % Acceleration noise in m/s^2
Q           = sigma_a^2*(B_f*B_f');
         
H     = [1 0 0 0; 0 1 0 0];
sigma_ra    = Range_res; %Range_res/1e3; %Range_res; % Range uncertainty
sigma_az    = deg2rad(beam_width/num_beams/4); %deg2rad(beam_width/num_beams/4)/1e3; %deg2rad(beam_width/num_beams/4); %0.010; % Azimuth uncertainty

v_max   = 30;
kappa   = 1;

% Initialise target and radar vectors
x_saved = zeros(4, duration-100, num_targets);
k_appear = zeros(1, num_targets);

y_saved = zeros(2, duration-100, num_targets, num_radars);
det_vec = zeros(duration-100, num_targets, num_radars);
targ_ind = zeros(duration-100, num_targets, num_radars);

clutter_returns(1:duration, 1:num_radars) = {[]};

for target = 1:num_targets
  c = [rand(2,1); randn(2,1)];
  x = c .* (initial_tsv + [range_extent(1,2)/2 range_extent(2,2)/2 0 0])';
  x = x + [range_extent(1,2)/4 range_extent(2,2)/4 0 0]';
  
  % Generate target
  for k = 1:duration-100
    a = sigma_a*randn(2,1);
    x = A_t*x + B_t*a;
    x_saved(:,k,target) = x;
  end

  % Specify target appearance discrete time
  k_appear(:,target) = randi(50,1,1);
end

if exist('debug', 'var') && debug && plot_on
  figure(f1);
  clf(f1);
  hold on;
  for target = 1:num_targets
    plot(x_saved(1,:,target),x_saved(2,:,target),'m');
  end
  axis(reshape(range_extent', 1, 4));
  hold off;
end

sim_az = zeros(num_radars, duration);
sim_dir = zeros(num_radars, duration);

sim_az(:,1) = radar_az;
sim_dir(:,1) = radar_dir;

radar_det_sym = {'x', '+'};

centre_az = zeros(1, num_radars);
start_az = zeros(1, num_radars);
stop_az = zeros(1, num_radars);
  
start_az_sorted = zeros(1, num_radars);
stop_az_sorted = zeros(1, num_radars);

centre_az_d = zeros(1, num_radars);
start_az_d = zeros(1, num_radars);
stop_az_d = zeros(1, num_radars);
  
start_az_sorted_d = zeros(1, num_radars);
stop_az_sorted_d = zeros(1, num_radars);

if exist('debug', 'var') && debug && plot_on
  for k=1:duration
    for radar = 1:num_radars
      % Radar beam location
      centre_az(radar) = deg2rad(sim_az(radar,k)); 
      start_az(radar) = centre_az(radar) - deg2rad(sim_dir(radar, k) * radar_bw(radar)/2);
      stop_az(radar) = centre_az(radar) + deg2rad(sim_dir(radar, k) * radar_bw(radar)/2);
      theta = deg2rad(radar_bw(radar)); %sim_dir(radar, k) * deg2rad(radar_bw(radar));
    
      if (stop_az(radar) < start_az(radar))
        start_az_sorted(radar) = stop_az(radar);
        stop_az_sorted(radar) = start_az(radar);
      else
        start_az_sorted(radar) = start_az(radar);
        stop_az_sorted(radar) = stop_az(radar);
      end

      % Generate Clutter
      clutter_returns(k,radar) = {genclutterSector2(beta_FA, start_az_sorted(radar), theta, radar_range(radar)-radar_range(radar)*radar_duty, radar_loc(:,radar), radar_range(radar)*radar_duty)};

      % Insert detections in clutter
      for target = 1:num_targets
        if (k >= k_appear(target)) && (k < (k_appear(target) + duration-100))
          x = x_saved(:,k-k_appear(target)+1,target);
          p = cart2pol(x(1:2)',radar_loc(:,radar)');

          target_az = p(2);
          target_range = abs(p(1));

          % Check if the target is in the beam of the radar
          if (start_az_sorted(radar) <= target_az) && (target_az <= stop_az_sorted(radar)) && (target_range <= radar_range(radar))
            det_vec(k-k_appear(target)+1,target,radar) = rand(1,1)>(1-P_D);
            % Check if the radar detects the target
            if det_vec(k-k_appear(target)+1,target,radar)==1
              % Generate radar measurement
              %y = polar_noise2(x(1:2)', sigma_ra, sigma_az, radar_loc(:,radar)')';
              y = polar_noise3(x(1:2)', sigma_ra, sigma_az, radar_loc(:,radar)', start_az_sorted(radar), stop_az_sorted(radar))';
              y_saved(:,k-k_appear(target)+1,target) = y;
              [r, c] = size(clutter_returns{k,radar});
              % Insert target detection into clutter measurements
              insertpt = randi(c+1,1,1);
              clutter_returns{k,radar} = [clutter_returns{k,radar}(:,1:insertpt-1) y clutter_returns{k,radar}(:,insertpt:c)];
              targ_ind(k-k_appear(target)+1,target,radar) = insertpt; % Stores index where target is
            else
              y_saved(:,k-k_appear(target)+1,target) = [NaN NaN]';
              targ_ind(k-k_appear(target)+1,target,radar) = NaN;
            end
          else
            det_vec(k-k_appear(target)+1,target,radar) = 0;
            y_saved(:,k-k_appear(target)+1,target) = [NaN NaN]';
            targ_ind(k-k_appear(target)+1,target,radar) = NaN;
          end
        end
      end
    end
    % Update beam start azimuth
    sim_dir(:,k+1) = sim_dir(:,k);
    sim_az(:,k+1) = sim_az(:,k) + radar_bp .* sim_dir(:,k+1);
    for radar = 1:num_radars
      % Change direction of the beam
      if (sim_az(radar,k) <= radar_az_min(radar))
        sim_dir(radar,k+1) = -sim_dir(radar,k+1);
        sim_az(radar,k+1) = radar_az_min(radar);
      elseif (sim_az(radar,k) >= radar_az_max(radar))
        sim_dir(radar,k+1) = -sim_dir(radar,k+1);
        sim_az(radar,k+1) = radar_az_max(radar);
      end
    end
  end

  for k=1:duration
    figure(f1);
    clf(f1);
    hold on;
    % Plot the target motion vectors
    for target = 1:num_targets
      plot(x_saved(1,:,target),x_saved(2,:,target),'m');
    end
    for radar = 1:num_radars
      % Plot the leading edge of the radar beam
      p = [radar_range(radar) deg2rad(sim_az(radar,k))];
      c = [radar_loc(:,radar)'; pol2cart(p, radar_loc(:,radar)')]';
      plot(c(1,:), c(2,:), 'g');
      
      % Plot the lagging edge of the radar beam
      p = [radar_range(radar) deg2rad(sim_az(radar,k)+sim_dir(radar,k)*radar_bw(radar))];
      c = [radar_loc(:,radar)'; pol2cart(p, radar_loc(:,radar)')]';
      plot(c(1,:), c(2,:), 'y');
      
      % Plot the radar detection circles (blind and max range).
      plot_circle(radar_loc(1,radar), radar_loc(2,radar), radar_range(radar)*radar_duty, 'y');
      plot_circle(radar_loc(1,radar), radar_loc(2,radar), radar_range(radar), 'g');
      
      % Plot the radar detections
      if ~isempty(clutter_returns{k,radar})
        plot(clutter_returns{k,radar}(1,:),clutter_returns{k,radar}(2,:),radar_det_sym{radar}); 
      end
    end
    axis(reshape(range_extent', 1, 4));
    drawnow;
    pause(0.1);
    hold off ;
  end
end

%% Sequential simulation

fail_tracks{num_radars} = [];
hist_tracks{num_radars} = [];
cand_tracks{num_radars} = [];
scan_returns{num_radars} = [];
fused_tracks(1).track_id = 0;
fused_tracks = fused_tracks(2:end);
track_id = 1;

nr_bsmf_miss = zeros(num_targets, 1);
nr_bsmf_poss = zeros(num_targets, 1);
nr_radar_poss = zeros(num_targets, num_radars);

nr_intercepts_miss = 0;
nr_radar_too_fast = 0;
nr_radar_out_of_bounds = 0;

for k=1:duration
  clear obs_covars;
  bsmf_poss = zeros(num_targets, 1);
  radar_poss = zeros(num_targets, num_radars);
  
  for radar=1:num_radars
    % Radar beam location
    centre_az(radar) = deg2rad(sim_az(radar,k));
    start_az(radar) = centre_az(radar) - deg2rad(sim_dir(radar,k) .* radar_bw(radar)/2);
    stop_az(radar) = centre_az(radar) + deg2rad(sim_dir(radar,k) .* radar_bw(radar)/2);
    theta(radar) = deg2rad(radar_bw(radar)); %sim_dir(radar, k) * deg2rad(radar_bw(radar));
    
    if (stop_az(radar) < start_az(radar))
      start_az_sorted(radar) = stop_az(radar);
      stop_az_sorted(radar) = start_az(radar);
    else
      start_az_sorted(radar) = start_az(radar);
      stop_az_sorted(radar) = stop_az(radar);
    end
    
    centre_az_d(radar) = rad2deg(centre_az(radar));
    start_az_d(radar) = rad2deg(start_az(radar));
    stop_az_d(radar) = rad2deg(stop_az(radar));
    
    start_az_sorted_d(radar) = rad2deg(start_az_sorted(radar));
    stop_az_sorted_d(radar) = rad2deg(stop_az_sorted(radar));

    % Generate Clutter
    clutter_returns(k,radar) = {genclutterSector2(beta_FA, start_az_sorted(radar), theta(radar), radar_range(radar)-radar_range(radar)*radar_duty, radar_loc(:,radar), radar_range(radar)*radar_duty)};
  end
  
  tgt_visible = zeros(num_targets, 1);
  tgt_in_beam = zeros(num_targets, num_radars);
      
  % Insert detections in clutter
  for target = 1:num_targets
    if (k >= k_appear(target)) && (k < (k_appear(target) + duration-100))
      tgt_visible(target) = 1;
      
      for radar=1:num_radars
        x = x_saved(:,k-k_appear(target)+1,target);
        p = cart2pol(x(1:2)',radar_loc(:,radar)');

        target_az = p(2);
        target_range = abs(p(1));

        % Check if the target is in the beam of the radar
        if (start_az_sorted(radar) <= target_az) && (target_az <= stop_az_sorted(radar)) && (radar_range(radar)*radar_duty <= target_range) && (target_range <= radar_range(radar))
          tgt_in_beam(target, radar) = 1;
        end
      end
      
      for radar=1:num_radars
        if tgt_in_beam(target, radar)
          if all(tgt_in_beam(target, :))
            P_D = 0.99; % hard coded for two radars and assuming Pd for monostatic and bistatic is 0.9
            bsmf_poss(target) = 1;
            nr_bsmf_poss(target) = nr_bsmf_poss(target) + 1;
          else
            P_D = 0.9;
            radar_poss(target, radar) = 1;
            nr_radar_poss(target, radar) = nr_radar_poss(target) + 1;
          end
          
          det_vec(k-k_appear(target)+1,target,radar) = rand(1,1)>(1-P_D);
          % Check if the radar detects the target
          if det_vec(k-k_appear(target)+1,target,radar)==1
            % Generate radar measurement
            %y = polar_noise2(x(1:2)', sigma_ra, sigma_az, radar_loc(:,radar)')';
            y = polar_noise3(x(1:2)', sigma_ra, sigma_az, radar_loc(:,radar)', start_az_sorted(radar), stop_az_sorted(radar))';
            y_saved(:,k-k_appear(target)+1,target) = y;
            [r, c] = size(clutter_returns{k,radar});
            % Insert target detection into clutter measurements
            insertpt = randi(c+1,1,1);
            clutter_returns{k,radar} = [clutter_returns{k,radar}(:,1:insertpt-1) y clutter_returns{k,radar}(:,insertpt:c)];
            targ_ind(k-k_appear(target)+1,target,radar) = insertpt; % Stores index where target is
          else
            y_saved(:,k-k_appear(target)+1,target) = [NaN NaN]';
            targ_ind(k-k_appear(target)+1,target,radar) = NaN;
          end
        else
          det_vec(k-k_appear(target)+1,target,radar) = 0;
          y_saved(:,k-k_appear(target)+1,target) = [NaN NaN]';
          targ_ind(k-k_appear(target)+1,target,radar) = NaN;
        end
      end
    end
  end
  
  %% Gather returns
  for radar=1:num_radars
    scan_returns{radar} = [scan_returns{radar} clutter_returns{k,radar}];

    % Calculate observation covariances for each return
    no_obs = length(scan_returns{radar}(1,:));
    for j = 1:no_obs
      obs_covars{radar}(:,:,j) = calc_obs_cov(scan_returns{radar}(:,j)',sigma_ra,sigma_az,radar_loc(:,radar)');
    end 
  end
  
  %% Radar Tracking
  % To get rid of the scan-by-scan track updates, coast tracks every time step.
  % Tracks are penalised if the target is in the beam and no detection is made.
  if 1%(mod(k,iter_per_sec)==0)
    for radar=1:num_radars
      % Radar beam location
      %centre_az(radar) = deg2rad(sim_az(radar,k));
      %start_az(radar) = centre_az(radar) - deg2rad(sim_dir(radar, k) * radar_bw(radar));
      %stop_az(radar) = centre_az(radar) + deg2rad(sim_dir(radar, k) * radar_bw(radar));

      %if (stop_az(radar) < start_az(radar))
      %  temp = start_az(radar);
      %  start_az(radar) = stop_az(radar);
      %  stop_az(radar) = temp;
      %  clear temp;
      %end
    
      % Initialise track variables
      clear unassigned_obs;

      [rt, ct]              = size(cand_tracks{radar});
      track_cnt             = 1;
      unassigned_obs        = scan_returns{radar};

      % Check which observations fall in gates of existing tracks
      for i = 1:ct
        track_x = cand_tracks{radar}(i).filter.x_hat(1:2);
        track_p = cart2pol(track_x', radar_loc(:,radar)');
        
        track_az = track_p(2);
        track_range = track_p(1);
        
        in_beam = ((start_az_sorted(radar)-sigma_az)<=track_az)&&(track_az<=(stop_az_sorted(radar)+sigma_az))&&((radar_range(radar)*radar_duty-sigma_ra)<=track_range)&&(track_range<=(radar_range(radar)+sigma_ra));
        
        [~, cr]  = size(unassigned_obs);
        % Compile list of observations in gate of track i
        obs = [];
        dist = [];
        ind_ret = [];
        for j = 1:cr
          dist_j = norm(cand_tracks{radar}(i).filter.x_pred(1:2)-unassigned_obs(:,j));
          %R      = calc_obs_cov(unassigned_obs(:,j),sigma_ra,sigma_az);
          igt    = cand_tracks{radar}(i).filter.is_in_gate(unassigned_obs(:,j), H, beta_FA);  % Should be total beta_FT + beta_NT, but beta_NT<<beta_FT
          if igt==1%dist_j<duration % CHANGE LATER TO COV GATE
            obs     = [obs unassigned_obs(:,j)];
            dist    = [dist dist_j];
            ind_ret = [ind_ret j];
          end
        end
        % Save predicted state and innovation covariance 
        % estimates for plotting (they will be changed as 
        % filter is updated)
        cand_tracks{radar}(i).prev_pred = H*cand_tracks{radar}(i).filter.x_pred;   % Remember prediction for plotting of gate
        cand_tracks{radar}(i).prev_S    = cand_tracks{radar}(i).filter.S;          % Remember innov cov for plotting of gate
        % Choose best observation in gate, if any
        if length(dist)>=1
          [best_dist, ind]    = min(dist);
          best_obs            = obs(:,ind);
          unassigned_obs = [unassigned_obs(:,1:ind_ret(ind)-1) unassigned_obs(:,ind_ret(ind)+1:cr)];
          R = calc_obs_cov(cand_tracks{radar}(i).filter.x_pred(1:2)',sigma_ra,sigma_az,radar_loc(:,radar)');
          % Update track filter of track i
          cand_tracks{radar}(i).filter  = cand_tracks{radar}(i).filter.iterate(best_obs, A_f, H, Q, R);
          cand_tracks{radar}(i).misscnt = 0;
        else % Coast filter
          R = calc_obs_cov(cand_tracks{radar}(i).filter.x_pred(1:2)',sigma_ra,sigma_az,radar_loc(:,radar)');
          cand_tracks{radar}(i).filter  = cand_tracks{radar}(i).filter.coast(A_f, H, Q, R, in_beam);
          if in_beam
            cand_tracks{radar}(i).misscnt = cand_tracks{radar}(i).misscnt + 1;
          end
        end 
      end

      % Start new tentative tracks for rest of observations
      [rr, cr]  = size(unassigned_obs);
      for i=ct+1:ct+cr
        range                          = norm(unassigned_obs(:,i-ct));
        init_pos_var                   = [(range*sigma_az)^2 (range*sigma_az)^2];
        init_R                         = calc_obs_cov(unassigned_obs(:,i-ct)',sigma_ra,sigma_az,radar_loc(:,radar)');
        cand_tracks{radar}(i).filter    = kalman2(4,unassigned_obs(:,i-ct), A_f, H, Q, init_R, init_pos_var,[v_max v_max], L, kappa, P_D, beta_FA);        
        cand_tracks{radar}(i).misscnt   = 0;    
        cand_tracks{radar}(i).confirmed = 0;   
        cand_tracks{radar}(i).tentative = 0;
        cand_tracks{radar}(i).prev_pred = unassigned_obs(:,i-ct);
        cand_tracks{radar}(i).prev_S    = cand_tracks{radar}(i-ct).filter.S;          % Remember innov cov for plotting of gate
        cand_tracks{radar}(i).prev_P    = cand_tracks{radar}(i-ct).filter.P(1:2,1:2); % Remember filter cov for plotting of gate
        cand_tracks{radar}(i).track_id  = track_id; track_id = track_id + 1;
      end
      % size(cand_tracks)
      % Delete candidate tracks which fail N/M test
      conf_cnt = 1;
      clear new_list;
      for i=1:ct+cr
        track_x = cand_tracks{radar}(i).filter.x_hat(1:2);
        track_p = cart2pol(track_x', radar_loc(:,radar)');
        
        track_az = track_p(2);
        track_range = track_p(1);
        
        if ((cand_tracks{radar}(i).filter.N==5)&&(cand_tracks{radar}(i).filter.M>=3))...
        || (cand_tracks{radar}(i).filter.N<5)...
        || (cand_tracks{radar}(i).misscnt<3)...
        || (cand_tracks{radar}(i).confirmed==1)...
        || ((cand_tracks{radar}(i).filter.N>5)&&(cand_tracks{radar}(i).misscnt<3))
          new_list(conf_cnt) = cand_tracks{radar}(i);
          % Set confirmed flag if M/N passed
          if ((new_list(conf_cnt).filter.N==5)&&(new_list(conf_cnt).filter.M>=3))
            new_list(conf_cnt).confirmed = 1;
            new_list(conf_cnt).misscnt = 0;
          elseif ((new_list(conf_cnt).confirmed==1)&&(new_list(conf_cnt).misscnt>3))
            cand_tracks{radar}(i).tentative = 1;
            new_list(conf_cnt).confirmed = 0;
            new_list(conf_cnt).misscnt = 0;
          elseif ((new_list(conf_cnt).confirmed==0)&&(new_list(conf_cnt).filter.N>5)&&(new_list(conf_cnt).misscnt==0))
            cand_tracks{radar}(i).tentative = 0;
            new_list(conf_cnt).confirmed = 1;
            new_list(conf_cnt).misscnt = 0;
          end
          conf_cnt = conf_cnt+1;
        elseif ((cand_tracks{radar}(i).filter.N>=5)&&(cand_tracks{radar}(i).filter.M>=3))
          hist_tracks{radar} = [hist_tracks{radar} cand_tracks{radar}(i)];
        else
          fail_tracks{radar} = [fail_tracks{radar} cand_tracks{radar}(i)];
        end
      end
      if (ct+cr)>0
        if exist('new_list','var')
          cand_tracks{radar} = new_list;
        else
          cand_tracks{radar} = [];
        end
        [rt, ct] = size(cand_tracks{radar});
      end
    end
  end
  
  %% Track association
  if 1%(mod(k,iter_per_sec)==0)
    ta_w = [];
    ta_idx = [];
    ta_rt = 1;
    ta_ct = 1;
    for i=1:length(cand_tracks{1})
      if (cand_tracks{1}(i).confirmed == 1) || ((cand_tracks{1}(i).filter.N>5)&&(cand_tracks{1}(i).misscnt<3))
        for j=1:length(cand_tracks{2})
          if (cand_tracks{2}(j).confirmed == 1) || ((cand_tracks{2}(j).filter.N>5)&&(cand_tracks{2}(j).misscnt<3))
            delta = cand_tracks{1}(i).filter.x_hat(1:2) - cand_tracks{2}(j).filter.x_hat(1:2);
            distance = sqrt(sum(delta .^ 2));
            ta_w(ta_rt, ta_ct) = 1/distance;
            ta_idx(1, ta_rt, ta_ct) = i;
            ta_idx(2, ta_rt, ta_ct) = j;
            ta_ct = ta_ct+1;
          end
        end
        ta_rt = ta_rt+1;
        ta_ct = 1;
      end
    end
    clear ta_a; clear ta_p;
    [ta_a, ta_p] = auction2(ta_w);
  end
  
  %% Track fusion
  if 1%(mod(k,iter_per_sec)==0)
    [tf_rt, tf_ct] = size(ta_w);
    for r=1:tf_rt
      clear c;
      c = find(ta_a(r,:)==1);
      if (exist('c','var') && ~isempty(c))
        if ((1/ta_p(c)) < 247) %50)
          clear f;
          i = ta_idx(1, r, c);
          j = ta_idx(2, r, c);
          
          delta = cand_tracks{1}(i).filter.x_pred(1:2) - cand_tracks{2}(j).filter.x_pred(1:2);
          distance = sqrt(sum(delta .^ 2));
          if (distance > 247) %50)
            continue;
          end
          
          if (~isempty(fused_tracks))
            f = find([fused_tracks(:).track_id]==cand_tracks{1}(i).track_id);
          end
          if (~exist('f','var') || isempty(f))
            f = length(fused_tracks) + 1;
            fused_tracks(f).track_id = cand_tracks{1}(i).track_id;
            fused_tracks(f).iteration = 0;
            fused_tracks(f).bsmf_poss = 0;
          end
          
          it = fused_tracks(f).iteration + 1;
          fused_tracks(f).iteration = it;

          % Keep track of what is going on
          fused_tracks(f).k(it) = k;
          fused_tracks(f).match_id(it) = cand_tracks{2}(j).track_id;
          
          fused_tracks(f).x_sens(:,1,it) = cand_tracks{1}(i).filter.x_hat;
          fused_tracks(f).P_sens(:,:,1,it) = cand_tracks{1}(i).filter.P;
          fused_tracks(f).x_sens(:,2,it) = cand_tracks{2}(j).filter.x_hat;
          fused_tracks(f).P_sens(:,:,2,it) = cand_tracks{2}(j).filter.P;

          if (~exist('fused_tracks(f).x_comb','var'))
            fused_tracks(f).x_comb(:,it) = zeros(size(cand_tracks{1}(i).filter.x_hat));
            fused_tracks(f).P_comb(:,:,it) = zeros(size(cand_tracks{1}(i).filter.P));
          end
          
          if (it == 1)
            [fused_tracks(f).x_comb(:,it), fused_tracks(f).P_comb(:,:,it)] = ...
                track_fusion( ...
                  fused_tracks(f).x_sens(:,:,it), fused_tracks(f).P_sens(:,:,:,it), ...
                  [], [], ...
                  [], [] ...
                );
          else
            [fused_tracks(f).x_comb(:,it), fused_tracks(f).P_comb(:,:,it)] = ...
                track_fusion( ...
                  fused_tracks(f).x_sens(:,:,it), fused_tracks(f).P_sens(:,:,:,it), ...
                  fused_tracks(f).x_comb(:,it-1), fused_tracks(f).P_comb(:,:,it-1), ...
                  fused_tracks(f).x_sens(:,:,it-1), fused_tracks(f).P_sens(:,:,:,it-1) ...
                );
          end
          
          % Predict next iteration
          fused_tracks(f).x_pred(:,it) = A_f * fused_tracks(f).x_comb(:,it);
          fused_tracks(f).P_pred(:,:,it) = A_f * fused_tracks(f).P_comb(:,:,it) * A_f' + Q;
          fused_tracks(f).confirmed = 1;
        end
      end
    end
  end
  
  fused_track_ids = zeros(num_radars, size(fused_tracks,2));
  for f = 1:length(fused_tracks)
    it = fused_tracks(f).iteration;
    % Check for expired fused tracks
    i = find([cand_tracks{1}(:).track_id]==fused_tracks(f).track_id);
    j = find([cand_tracks{2}(:).track_id]==fused_tracks(f).match_id(it));
    if (~isempty(i) && ~isempty(j))
      fused_tracks(f).confirmed = (cand_tracks{1}(i).confirmed | cand_tracks{1}(i).tentative) ... 
                                & (cand_tracks{2}(j).confirmed | cand_tracks{2}(j).tentative);
    else
      fused_tracks(f).confirmed = 0;
    end

    fused_track_ids(1,f) = fused_tracks(f).track_id;
    fused_track_ids(2,f) = fused_tracks(f).match_id(it);
  end
  
  %% Scheduling
    
  perform_scheduling = zeros(1, num_radars);
	at_intercept = zeros(1, num_radars);
  at_scan_bound = zeros(1, num_radars);
  
  for radar = 1:num_radars  
    if exist('azimuth_rates', 'var') %then azimuth_intercepts must also exist
      if ((start_az_sorted_d(radar) <= azimuth_intercepts(radar, rate_index)) ...
      && (stop_az_sorted_d(radar) >= azimuth_intercepts(radar, rate_index)) ...
      && (azimuth_rates(radar, rate_index) == 0)) ...
      || ((-1e-12 <= (azimuth_intercepts(radar, rate_index) - centre_az_d(radar))) ...
      && ((azimuth_intercepts(radar, rate_index) - centre_az_d(radar)) <= 1e-12))
        at_intercept(radar) = 1; % can be target or scan bound
        radar_bp(radar) = 0;
        azimuth_rates(radar, rate_index) = 0;
        azimuth_error(radar, rate_index) = azimuth_intercepts(radar, rate_index) - centre_az_d(radar);
      end
      
      % detect errors in the scheduling
      if sim_dir(radar,k) > 0
       if start_az_sorted_d(radar) > azimuth_intercepts(radar, rate_index)
         warning('missed an intercept point %f > %f', start_az_sorted_d(radar), azimuth_intercepts(radar, rate_index));
         nr_intercepts_miss = nr_intercepts_miss + 1;
       end
      elseif sim_dir(radar,k) < 0
       if stop_az_sorted_d(radar) < azimuth_intercepts(radar, rate_index)
         warning('missed an intercept point %f < %f', stop_az_sorted_d(radar), azimuth_intercepts(radar, rate_index));
         nr_intercepts_miss = nr_intercepts_miss + 1;
       end
      end
    end
  
    if (stop_az_d(radar) <= radar_az_min(radar))
      perform_scheduling(radar) = 1;
      at_scan_bound(radar) = 1; % definitely at scan bound
      radar_bp(radar) = 0;
      if exist('azimuth_rates', 'var')
        azimuth_rates(radar, rate_index) = 0;
      end
    elseif (stop_az_d(radar) >= radar_az_max(radar))
      perform_scheduling(radar) = 1;
      at_scan_bound(radar) = 1; % definitely at scan bound
      radar_bp(radar) = 0;
      if exist('azimuth_rates', 'var')
        azimuth_rates(radar, rate_index) = 0;
      end
    end
    
    if (stop_az_sorted_d(radar) < radar_az_min(radar))
      warning('radar past scan bounds %f < %f', stop_az_sorted_d(radar), radar_az_min(radar));
      nr_radar_out_of_bounds = nr_radar_out_of_bounds + 1;
    elseif (start_az_sorted_d(radar) > radar_az_max(radar))
      warning('radar past scan bounds %f > %f', start_az_sorted_d(radar), radar_az_max(radar));
      nr_radar_out_of_bounds = nr_radar_out_of_bounds + 1;
    end
    
    if nimble_scheduling   
      % Nimble scheduling:
      % - Add code here to enter scheduling on each intercept.
      %   Can we move up the code after the scheduling is performed?
      % - Need a flag that determines if we have reach the scan ends or a
      %   target.
      if ~perform_scheduling(radar)
        perform_scheduling(radar) = at_intercept(radar);
      end
    end
  end
  
  if all(at_intercept) && ~all(at_scan_bound)
    f = find([fused_tracks(:).track_id] == sched_bsmf_tracks(rate_index-1), 1);
    fused_tracks(f).bsmf_poss = fused_tracks(f).bsmf_poss + 1;
  end
  
  %if all(perform_scheduling) && ~all(at_scan_bound)
  %  delta = azimuth_intercepts(:, rate_index+1) - azimuth_intercepts(:, rate_index);
  %  for radar = 1:num_radars
  %    if (delta(radar) == 0)
  %      perform_scheduling(radar) = 0;
  %    end
  %  end
  %end
  
  %if (any(at_scan_bound) && ~all(at_scan_bound)) ...
  %|| (any(at_intercept) && ~all(at_intercept))
  %  if any(at_scan_bound)
  %    warning('one radar is at scan fast bounds too quickly: [%d %d]', at_scan_bound(1), at_scan_bound(2));
  %  end
  %  if any(at_intercept)
  %    warning('one radar is at an intercept too quickly: [%d %d]', at_intercept(1), at_intercept(2));
  %  end
  %  nr_radar_too_fast = nr_radar_too_fast + 1;
  %end
    
  % Predict azimuth bearing to each target & contact (to confirm tracks)
  % 1) fused tracks we need to predict forward similar to kalman filter
  % 2) single radar tracks use kalman filter prediction.
  if all(perform_scheduling)    
    if all(at_scan_bound)
      for radar = 1:num_radars
        % Change direction of the beam
        if (stop_az_d(radar) <= radar_az_min(radar))
          sim_dir(radar,k+1) = -sim_dir(radar,k);
          sim_az(radar,k+1) = radar_az_min(radar);
        elseif (stop_az_d(radar) >= radar_az_max(radar))
          sim_dir(radar,k+1) = -sim_dir(radar,k);
          sim_az(radar,k+1) = radar_az_max(radar);
        else
          error('impossible to not be at scan bound');
        end
      end
    else
      sim_dir(:,k+1) = sim_dir(:,k);
      sim_az(:,k+1) = azimuth_intercepts(:, rate_index);
    end
    
    if all(at_scan_bound)
      rate_index = 1;
      prev_bsmf_tracks = [];
      prev_azimuth_intercepts = [];
      prev_sched_iteration = [];
      azimuth_error = [];
      sched_k = k;
    else
      prev_bsmf_tracks = sched_bsmf_tracks;%(1:rate_index-1);
      prev_azimuth_intercepts = azimuth_intercepts;%(:, 1:rate_index);
      prev_sched_iteration = sched_iteration;%(:, 1:rate_index);
      sched_k = [sched_k k];
    end
    
    % Debug code, remove if no longer necessary
    if (length(prev_bsmf_tracks)==5) && all(prev_bsmf_tracks == [39 44 8 9 73])
      prev_bsmf_tracks = prev_bsmf_tracks;
    end
    
    % High-level scheduler
    sched_track_ids = {};
    for radar = 1:num_radars
      sched_track_ids{radar} = zeros(Ang_bins, 1);
    end
    
    sched_priority_queue = [];
    q = 1;
    
    for f = 1:length(fused_tracks)
      if (fused_tracks(f).confirmed == 1)
        it = fused_tracks(f).iteration;
        
        % Nimble scheduling: ignore tracks that have already been handled
        t1 = find(prev_bsmf_tracks(1:rate_index-1) == fused_tracks(f).track_id);
        if ~isempty(t1)
          continue; % already visited
        end
        
        if predict_intercepts_upper
          for radar = 1:num_radars
            radar_track_pol(radar, :) = cart2pol(fused_tracks(f).x_comb(1:2,it)', radar_loc(:,radar)');
            radar_track_az(radar) = rad2deg(radar_track_pol(radar, 2));
          end

          delta_az = radar_track_az(:) - sim_az(:,k+1);
          if any(sign(delta_az) ~= sim_dir(:,k+1)) && all(sign(delta_az)) && all(abs(delta_az) > radar_bw/2)
            continue; % can no longer reach this target
          end
          
          if any(delta_az > radar_bw/2)
            [~, fast_radar] = max(abs(delta_az));

            [k_int, x_k_int] = determine_intercept_iteration(sim_az(fast_radar, k), sim_dir(fast_radar, k+1) * beam_az_prog, radar_bw(fast_radar), A_f, fused_tracks(f).x_comb(:,it), radar_loc(:,fast_radar));
            if (k_int > 90)
              continue; % impossible to reach this target (it may be out of the region of regard)
            end
          else
            k_int = 0;
            x_k_int = fused_tracks(f).x_comb(:,it);
          end
          
          fused_tracks(f).x_sched(:,it) = x_k_int;
        else
          fused_tracks(f).x_sched(:,it) = fused_tracks(f).x_comb(:,it);
        end;
        
        for radar = 1:num_radars
          fused_track_pol(radar, :) = cart2pol(fused_tracks(f).x_sched(1:2,it)', radar_loc(:,radar)');
          fused_track_az(radar) = rad2deg(fused_track_pol(radar, 2));
        end
        
        % Nimble scheduling: ignore tracks that can no longer be reached
        skip = 0;
        for radar = 1:num_radars  
          if (fused_track_az(radar) < (radar_az_min(radar) - radar_bw(radar)/2)) ...
          || (fused_track_az(radar) > (radar_az_max(radar) + radar_bw(radar)/2))
            skip = 1;
            break;
          end
          
          if sim_dir(radar, k+1) > 0
            if fused_track_az(radar) <= start_az_sorted_d(radar)
              skip = 1;
              break;
            end
          elseif sim_dir(radar, k+1) < 0
            if stop_az_sorted_d(radar) <= fused_track_az(radar)
              skip = 1;
              break;
            end
          end
        end
        if skip
          continue
        end
        
        sched_priority_queue(q, 1) = fused_tracks(f).track_id;
        % TODO: Should predict the covariance at the time of intercept.
        %sched_priority_queue(q, 2) = cov_area(fused_tracks(f).P_pred(1:2,1:2,it),2);
        t1 = find([cand_tracks{1}(:).track_id] == fused_tracks(f).track_id);
        t2 = find([cand_tracks{2}(:).track_id] == fused_tracks(f).match_id(it));
        if ~isempty(t1)
          sched_priority_queue(q, 2) = cov_area(cand_tracks{1}(t1).filter.P_pred(1:2,1:2),2);
          if ~isempty(t2)
            sched_priority_queue(q, 2) = sched_priority_queue(q, 2) + cov_area(cand_tracks{2}(t2).filter.P_pred(1:2,1:2),2);
          else
            sched_priority_queue(q, 2) = Inf;
          end
        else
          sched_priority_queue(q, 2) = Inf;
        end
        q = q + 1;
        
        for radar = 1:num_radars
          az_bin = round(abs(fused_track_az(radar)) * (Ang_bins-1) / scan_angle) + 1;
          
          if ((1 <= az_bin) && (az_bin <= Ang_bins))
            tn = find(sched_track_ids{radar}(az_bin,:) == 0, 1);
            if isempty(tn)
              sched_track_ids{radar}(az_bin, end+1) = fused_tracks(f).track_id;
            else
              sched_track_ids{radar}(az_bin, tn) = fused_tracks(f).track_id;
            end
          else
            sched_priority_queue = sched_priority_queue(1:end-1, :);
            q = q - 1;
            break;
          end
        end
      end
    end
    
    if (length(sched_priority_queue)>1)
      [status, sched_bsmf_tracks] = BsMf_Schedule(sched_priority_queue, sched_track_ids{1}, sched_track_ids{2}, param);
    
      if ~status
        sched_bsmf_tracks = [];
      end
    else
      sched_bsmf_tracks = [];
    end
    
    if sim_dir(1, k+1) < 0
      sched_bsmf_tracks = fliplr(sched_bsmf_tracks);
    end
    sched_bsmf_tracks = [prev_bsmf_tracks(1:rate_index-1) sched_bsmf_tracks];
    
    % Debug code, remove if no longer necessary
    if (length(sched_bsmf_tracks)==5) && all(sched_bsmf_tracks == [39 44 8 9 73])
      sched_bsmf_tracks = sched_bsmf_tracks;
    end
    
    % Low-level scheduler (determine the control parameters)
    azimuth_intercepts = zeros(num_radars, length(sched_bsmf_tracks)+2);
    sched_iteration = zeros(1, length(sched_bsmf_tracks)+2);

    if sim_dir(1, k+1) < 0
      azimuth_intercepts(:,1) = [scan_angle; -scan_angle];
    else
      azimuth_intercepts(:,end) = [scan_angle; -scan_angle];
    end

    if ~isempty(prev_sched_iteration)
      sched_iteration(:,1) = prev_sched_iteration(:,1) - prev_sched_iteration(:,rate_index);
    end
    
    for t=1:length(sched_bsmf_tracks)
      % Nimble scheduler: populate already visited targets
      if t < rate_index
        azimuth_intercepts(:,t+1) = prev_azimuth_intercepts(:,t+1);
        sched_iteration(:,t+1) = prev_sched_iteration(:,t+1) - prev_sched_iteration(:,rate_index);
        continue;
      end
      
    	f = find([fused_tracks(:).track_id] == sched_bsmf_tracks(t), 1);
      it = fused_tracks(f).iteration;
      
      % Determine intercept iteration
      if predict_intercepts_lower
        % Predict forward to the intercept point of the prior target
        x_k_prev = A_f^sched_iteration(t) * fused_tracks(f).x_comb(:,it);
        
        for radar = 1:num_radars
          radar_track_pol(radar, :) = cart2pol(x_k_prev(1:2)', radar_loc(:,radar)');
          radar_track_az(radar) = rad2deg(radar_track_pol(radar, 2));
        end

        delta_az = radar_track_az(:) - azimuth_intercepts(:,t);
        sign_delta = sign(delta_az);
        eq_sign = (sign_delta == sim_dir(:,k+1));
        if ~all(eq_sign) && all(sign_delta)
          fast_radar = find(eq_sign);
        else
          [~, fast_radar] = max(abs(delta_az));
        end
        
        if any(eq_sign)
          % Predict forward to the intercept point of the current target
          [k_int, x_k_int] = determine_intercept_iteration(azimuth_intercepts(fast_radar, t), sim_dir(fast_radar, k+1) * beam_az_prog, radar_bw(fast_radar), A_f, x_k_prev, radar_loc(:,fast_radar));
          if (k_int > 90)
            %error('Unreachable target (intercept iteration)!'); % error should be emitted here
            fused_tracks(f).x_sched(:,it) = x_k_prev;
            sched_iteration(t+1) = sched_iteration(t);
            % we probably need to delete this target from the schedule?
          else
            fused_tracks(f).x_sched(:,it) = x_k_int;
            sched_iteration(t+1) = sched_iteration(t) + k_int;
          end
        else
          fused_tracks(f).x_sched(:,it) = x_k_prev;
          sched_iteration(t+1) = sched_iteration(t);
          % if the azimuth delta is larger than the beam width for either
          % radar we must delete this target from the schedule.
        end
      else
        fused_tracks(f).x_sched(:,it) = fused_tracks(f).x_pred(:,it);
      end
      
      for radar=1:num_radars
        fused_track_pol(radar, :) = cart2pol(fused_tracks(f).x_sched(1:2,it)', radar_loc(:,radar)');
        fused_track_az(radar) = rad2deg(fused_track_pol(radar, 2));
        azimuth_intercepts(radar,t+1) = fused_track_az(radar);
      end
    end
        
    azimuth_int_fix_close = azimuth_intercepts;
    azimuth_intercepts = fix_close_targets(azimuth_intercepts, radar_bw);    
    
    for t=1:length(sched_bsmf_tracks)
      if t < rate_index
        azimuth_intercepts(:,t+1) = prev_azimuth_intercepts(:,t+1);
        sched_iteration(:,t+1) = prev_sched_iteration(:,t+1) - prev_sched_iteration(:,rate_index);
        continue;
      end
    end
    
    % Nimble scheduling: fix targets in beam but that lie behind the
    % current resting position.
    azimuth_int_fix_beam = azimuth_intercepts;
    azimuth_intercepts = fix_targets_in_beam(azimuth_intercepts, azimuth_intercepts(:, rate_index), radar_bw, sim_dir(:,k+1), rate_index);
    
    % All scheduling: remove targets that are impossible from the sequence.
    sched_bsmf_tracks_imp = sched_bsmf_tracks;
    azimuth_intercepts_imp = azimuth_intercepts;
    sched_iteration_imp = sched_iteration;
    
    [sched_bsmf_tracks, azimuth_intercepts, sched_iteration] = remove_impossible_targets(sched_bsmf_tracks, azimuth_intercepts, sched_iteration);
    
    azimuth_rates = calc_positioner_rates(azimuth_intercepts, beam_az_prog);
    azimuth_rates = [azimuth_rates zeros(2, 1)];
    
    if ~isempty(prev_azimuth_intercepts)
      for t=1:rate_index
        azimuth_rates(:,t) = 0;
      end
    end
  end
  
  % Low-level scheduler (control the radar positioners)
  if exist('azimuth_rates', 'var')
    if all(at_intercept) || ~any(azimuth_rates(:,rate_index))
      if  ~any(at_scan_bound)
        f = find([fused_tracks(:).track_id] == sched_bsmf_tracks(rate_index-1));
        it = fused_tracks(f).iteration;
        x_f = fused_tracks(f).x_comb(1:2, it);

        min_distance = Inf;
        min_target = 0;
        for target=1:num_targets
          if k<k_appear(target)
            delta_x = x_f - x_saved(1:2, 1, target);
          elseif k>=k_appear(target)&&k<(k_appear(target)+duration-100)
            delta_x = x_f - x_saved(1:2, k-k_appear(target)+1, target);
          else
            delta_x = x_f - x_saved(1:2, end, target);
          end
          distance = sqrt(sum(delta_x .^ 2));
          if distance < min_distance
            min_distance = distance;
            min_target = target;
          end
        end

        if (min_target > 0) && (min_distance <= 247)
          if tgt_visible(min_target) && ~bsmf_poss(min_target)
            nr_bsmf_miss(min_target) = nr_bsmf_miss(min_target) + 1;
            warning('missed %d multi-static measurement(s)', sum(nr_bsmf_miss));
          end
        end
      end
      if (rate_index < size(azimuth_rates,2))
        rate_index = rate_index + 1;
      end
      radar_bp = azimuth_rates(:,rate_index);
    else
      radar_bp = azimuth_rates(:,rate_index);
    end
  end
  
  % Update beam start azimuth
  if ~all(perform_scheduling)
    sim_dir(:,k+1) = sim_dir(:,k);
    sim_az(:,k+1) = sim_az(:,k) + radar_bp .* sim_dir(:,k+1);
  else
    sim_dir(:,k+1) = sim_dir(:,k+1);
    sim_az(:,k+1) = sim_az(:,k+1) + radar_bp .* sim_dir(:,k+1);
  end
  
  %% Plot routines
  if plot_on %(mod(k,iter_per_sec)==0)
    clf(f1);
    figure(f1);
    hold on;
    % Plot the target motion vectors
    for target = 1:num_targets
      plot(x_saved(1,:,target),x_saved(2,:,target),'m');
      if k>=k_appear(target)&&k<(k_appear(target)+duration-100)
        plot(x_saved(1,k-k_appear(target)+1,target),x_saved(2,k-k_appear(target)+1,target),'mo');
      end
    end
    for radar = 1:num_radars
      % Plot the radar symbology
      % Plot the leading edge of the radar beam
      p = [radar_range(radar) start_az(radar)];
      c = [radar_loc(:,radar)'; pol2cart(p, radar_loc(:,radar)')]';
      plot(c(1,:), c(2,:), 'g');

      % Plot the lagging edge of the radar beam
      p = [radar_range(radar) stop_az(radar)];
      c = [radar_loc(:,radar)'; pol2cart(p, radar_loc(:,radar)')]';
      plot(c(1,:), c(2,:), 'y');

      % Plot the radar detection circles (blind and max range).
      plot_circle(radar_loc(1,radar), radar_loc(2,radar), radar_range(radar)*radar_duty, 'y');
      plot_circle(radar_loc(1,radar), radar_loc(2,radar), radar_range(radar), 'g');

      % Plot the radar detections
      if ~isempty(scan_returns{radar})
        plot(scan_returns{radar}(1,:),scan_returns{radar}(2,:),radar_det_sym{radar}); 
        no_obs = length(scan_returns{radar}(1,:));
        for j = 1:no_obs
          cov_ellipse2(obs_covars{radar}(:,:,j),scan_returns{radar}(:,j),2,'')
        end
      end
      % Plot the radar tracks
      [~, ct] = size(cand_tracks{radar});
      for i = 1:ct
        % Plot only the confirmed radar tracks (for speedup)
        if (cand_tracks{radar}(i).confirmed==1)||((cand_tracks{radar}(i).filter.N>5)&&(cand_tracks{radar}(i).misscnt<3))
          plot(cand_tracks{radar}(i).filter.x_hat(1),cand_tracks{radar}(i).filter.x_hat(2),'go');
          cov_ellipse2(cand_tracks{radar}(i).filter.P(1:2,1:2),cand_tracks{radar}(i).filter.x_hat,2,'g');
          plot(cand_tracks{radar}(i).prev_pred(1),cand_tracks{radar}(i).prev_pred(2),'r.');
          cov_ellipse2(cand_tracks{radar}(i).prev_S,cand_tracks{radar}(i).prev_pred,sqrt(cand_tracks{radar}(i).filter.G_0),'r');
          if (cand_tracks{radar}(i).confirmed==1)
            plot(cand_tracks{radar}(i).filter.x_vec(1,:),cand_tracks{radar}(i).filter.x_vec(2,:));
          elseif ((cand_tracks{radar}(i).filter.N>5)&&(cand_tracks{radar}(i).misscnt<3))
            plot(cand_tracks{radar}(i).filter.x_vec(1,:),cand_tracks{radar}(i).filter.x_vec(2,:), 'g');
          end
        end
      end
      % Plot the radar detections for targets
      for target = 1:num_targets
        if k>=k_appear(target)&&k<(k_appear(target)+duration-100)&&~isnan(targ_ind(k+1-k_appear(target),target,radar))&&~isempty(scan_returns{radar})
          if det_vec(k+1-k_appear(target),target,radar)==1
            plot(scan_returns{radar}(1,targ_ind(k+1-k_appear(target),target,radar)),scan_returns{radar}(2,targ_ind(k+1-k_appear(target),target,radar)),'ro');
          end
        end
      end
      % Plot historical tracks
      [~, ht] = size(hist_tracks{radar});
      for i = 1:ht
        if (hist_tracks{radar}(i).confirmed == 1) || (hist_tracks{radar}(i).filter.N > 6)
          plot(hist_tracks{radar}(i).filter.x_vec(1,:),hist_tracks{radar}(i).filter.x_vec(2,:), 'g');
        end
      end
    end
    % Plot fused tracks
    [~, ft] = size(fused_tracks);
    for f = 1:ft
      if 1%(fused_tracks(f).confirmed == 1)
        cov_ellipse2(fused_tracks(f).P_comb(1:2,1:2,end),fused_tracks(f).x_comb(:,end),2,'c');
        plot(fused_tracks(f).x_comb(1,:),fused_tracks(f).x_comb(2,:),'c');
      end
    end
    % Scheduling plots
    for f = 1:length(fused_tracks)
      if isfield(fused_tracks(f), 'x_sched') && all(size(fused_tracks(f).x_sched))
        sched_x = [fused_tracks(f).x_comb(1,end) fused_tracks(f).x_sched(1,end)];
        sched_y = [fused_tracks(f).x_comb(2,end) fused_tracks(f).x_sched(2,end)];
        plot(sched_x, sched_y, 'c--');
      end
      
      if (fused_tracks(f).confirmed == 1)
        it = fused_tracks(f).iteration;
        for radar = 1:num_radars
          c = [radar_loc(:,radar)'; fused_tracks(f).x_comb(1:2,it)']';
          plot(c(1,:), c(2,:), 'c');
        end
      end
    end

    for radar = 1:num_radars
      for t = 1:length(cand_tracks{radar})
        if (cand_tracks{radar}(t).confirmed == 1)
          f = find(fused_track_ids(radar,:)==cand_tracks{radar}(t).track_id);
          if isempty(f) || ~any([fused_tracks(f).confirmed])
            c = [radar_loc(:,radar)'; cand_tracks{radar}(t).filter.x_hat(1:2)']';
            plot(c(1,:), c(2,:), 'b');
          end
        end
      end
    end
    % Complete the plot
    axis(reshape(range_extent', 1, 4));
    axis square;
    drawnow;
    pause(0.01);
    hold off;
  end
  
  %% Sort the candidate tracks
  if 1%(mod(k,iter_per_sec)==0)
    for radar=1:num_radars
      if ~isempty(cand_tracks{radar})
        conf = [cand_tracks{radar}.confirmed];
        filter = [cand_tracks{radar}.filter];
        M = [filter.M];
        N = [filter.N];
        G_0 = [filter.G_0];
        [~, ind] = sortrows([conf' M' G_0' N'], [-1 -2 3 -4]);
        cand_tracks{radar} = cand_tracks{radar}(ind);
      end
    end
  end
  
  %% Reset the scan returns
  if 1%(mod(k,iter_per_sec)==0)
    for radar=1:num_radars
      scan_returns{radar} = [];
    end
  end
end


